#include "ConvPCD.h"
#include "picojson.h"
#include "api.h"
#include <string>

static const char *HeadPCD = "VERSION .7\nFIELDS x y z\nSIZE 8 8 8\nTYPE F F F\nCOUNT 1 1 1 \nWIDTH %d \nHEIGHT %d \nVIEWPOINT 0 0 0 1 0 0 0 \nPOINTS %d DATA ascii \n";
static const char *LinePCD = "%.4f %.4f %.4f\n";

int Depth2PCD(uint16_t * depth, char *pcd, char *intrinsics)
{
    int retLenData = 0;
    char *pBuff = pcd;
    float fx = 1.0; // 焦距x
    float fy = 1.0; // 焦距y
    float cx = 0.0; // 主点x
    float cy = 0.0; // 主点y
    float x, y, z;
    uint16_t *pDepthData = depth;
    int mImgWidth = 640, mImgHeight = 480;

    picojson::value mIntrinsicsJson;
    picojson::object mObj;
    if (!picojson::parse(mIntrinsicsJson, std::string(intrinsics)).empty())
    {
        return -1;
    }
    mObj = mIntrinsicsJson.get<picojson::object>();
    mImgWidth = atoi(mObj["width"].to_str().data());
    mImgHeight = atoi(mObj["height"].to_str().data());
    fx = atof(mObj["f_x"].to_str().data());
    fy = atof(mObj["f_y"].to_str().data());
    cx = atof(mObj["c_x"].to_str().data());
    cy = atof(mObj["c_y"].to_str().data());

    //head
    sprintf(pBuff , HeadPCD, mImgWidth, mImgHeight, mImgWidth * mImgHeight);
    retLenData += strlen(pBuff);
    pBuff += strlen(pBuff);
    for (int i = 0; i < mImgHeight; i++)
    {
        for (int j = 0; j < mImgWidth; j++)
        {
            if (*pDepthData > 0 && *pDepthData < 0xFFF0)
            {
                x = (j - cx) * (*pDepthData) / fx;
                y = (i - cy) * (*pDepthData) / fy;
                z = *pDepthData;
            }
            else
            {
                x = 0.0;
                y = 0.0;
                z = 0.0;
            }
            pDepthData++;
            sprintf(pBuff, LinePCD, x, y, z);
            retLenData += strlen(pBuff);
            pBuff += strlen(pBuff);
        }
    }


    return retLenData;
}